#include "SpatialTree.h"

#define USE_BENCHMARK

#include "Benchmark.h"

CSpatialTree::CSpatialTree()
{
	Null();
}

CSpatialTree::~CSpatialTree()
{
	Release();
}

void CSpatialTree::Null()
{
	m_pPos = NULL;
	m_pPosIdxSwzld = NULL;

	m_nMaxDepth     = 8;
	m_nMaxPrim      = 4;
	m_nPrimExplode  = 16;
	m_nNodeIndex    = 0;
	m_nPrimitive    = 0;
	m_nTree         = 0;
	m_nTraversalIdx = 0;
	m_nFace         = 0;
}

float CSpatialTree::EvaluateSurfaceAreaHeuristics(float probabilityLeft, float probabilityRight, int32 triangleCountLeft, int32 triangleCountRight, float Ct, float Cb)
{
	return (probabilityLeft * triangleCountLeft + probabilityRight * triangleCountRight) * Ct + Cb * 2; // add Cb * 2 because intersect with 2 boxes
}

float CSpatialTree::EvaluateMedianHeuristics(int32 triangleCountLeft, int32 triangleCountRight, float Ct, float Cb)
{
	return max(triangleCountLeft, triangleCountRight) * Ct + Cb * 2;
}

void CSpatialTree::Create(sf4Buffer * pPos, ui_Buffer * pPosIdxSwzld, si_Buffer * pPosIdx)
{
	m_pPos = pPos;                   // need to save pointers
	m_pPosIdxSwzld = pPosIdxSwzld;   // need to save pointers
	m_pPosIdx = pPosIdx;             // need to save pointers 

	m_nFace = pPosIdxSwzld->size() / 3;

	m_nMaxDepth = 8 + 1.5f * ((uint32)logf(m_nFace));

	for (uint32 iAxis = X; iAxis < W; iAxis++)
	{
		m_histopyramid [iAxis].CreateBin(5,     BUFFER_H_AND_D);
		m_PrimitiveBI  [iAxis].Create   (m_nFace, BUFFER_H_AND_D);       // contains m_nFace BBoxes for each triangle separatly for XYZ
	}

	m_primIdx.Create(m_nFace * m_nPrimExplode, BUFFER_H_AND_D, true);
	m_treeIdx.Create(m_nFace * m_nPrimExplode, BUFFER_H_AND_D, false);

	m_Interval.Create (XYZ*((1<<(m_nMaxDepth+1))-1), BUFFER_H_AND_D);
	m_Tree.Create     (     (1<<(m_nMaxDepth+1))-1,  BUFFER_H_AND_D);
	m_KdTree.Create   (     (1<<(m_nMaxDepth+1))-1,  BUFFER_H_AND_D);
	m_BVHTree.Create  (2 * ((1<<(m_nMaxDepth+1))-1), BUFFER_H_AND_D);
}

void CSpatialTree::Release()
{
	Null();

	for (uint32 iAxis = X; iAxis < W; iAxis++)
	{
		m_histopyramid[iAxis].Release();
	}
}

void CSpatialTree::PrimitiveAABB(uint8 exeFlags)
{
	CBenchmark primaabbBench;

	if (exeFlags == EXEC_H)
	{
		int3 i;

		uint32 * pPosIdx = m_pPosIdxSwzld->hPtrR();
		float4 * pPos    = m_pPos->hPtrW();

		float2 * pMmX = m_PrimitiveBI[X].hPtrR();
		float2 * pMmY = m_PrimitiveBI[Y].hPtrR();
		float2 * pMmZ = m_PrimitiveBI[Z].hPtrR();

		primaabbBench.Start("PrimitiveAABB");

		for (uint32 iPrim = 0; iPrim < m_nFace; iPrim++)
		{
			i.x = pPosIdx[iPrim+m_nFace*0];
			i.y = pPosIdx[iPrim+m_nFace*1];
			i.z = pPosIdx[iPrim+m_nFace*2];

			float3 v0 = xyz(pPos[i.x]);
			float3 v1 = xyz(pPos[i.y]);
			float3 v2 = xyz(pPos[i.z]);

			float3 mn = min(min(v0, v1), v2);
			float3 mx = max(max(v0, v1), v2);

			pMmX[iPrim] = f2(mn.x, mx.x);
			pMmY[iPrim] = f2(mn.y, mx.y);
			pMmZ[iPrim] = f2(mn.z, mx.z);
		}

		primaabbBench.End();
	}

	if (exeFlags == EXEC_D)
	{
	}
}

void CSpatialTree::Histogram(uint8        exeFlags, 
														 uint32     * pPrim, 
														 uint32       nPrim,
														 SInterval  * pInterval)
{
	if (exeFlags == EXEC_H)
	{
		int32 leftIndex = 0, rightIndex = 0;		

		for (uint32 iAxis = X; iAxis <= Z; iAxis++)
		{
			SInterval * pBI = (SInterval *) m_PrimitiveBI[iAxis].hPtrR();
			float2 * pHistogram = m_histopyramid[iAxis].histogram.hPtrR();

			int32   nBin = m_histopyramid[iAxis].nBin();

			SInterval interval = pInterval[iAxis];
			float      length   = interval.length();

			memset(pHistogram, 0, sizeof(float2) * nBin);

			for (uint32 iPrim = 0; iPrim < nPrim; iPrim++)
			{
				uint32 PrimitiveIndex = pPrim[iPrim];

				SInterval bounding_interval = pBI[PrimitiveIndex];

				leftIndex  = (bounding_interval.start - interval.start) / length * nBin;
				rightIndex = (bounding_interval.end   - interval.start) / length * nBin;

				//if (leftIndex < nBin && leftIndex >= 0)
				if (leftIndex < nBin)
					pHistogram[max(leftIndex, (int32) 0)].x += 1;

				//if (rghtIndex < nBin && rghtIndex >= 0)
				if (rightIndex >= 0)
					pHistogram[min(rightIndex, (int32) nBin-1)].y += 1;
			}
			m_histopyramid[iAxis].Scan(exeFlags, BUFFER_R);

			float2 * pScan = m_histopyramid[iAxis].scan.hPtrR();
			// reverse order of right sides
			for (int32 iBin = 0; iBin <= nBin; iBin++)
				pScan[iBin].y = pScan[nBin].y - pScan[iBin].y; 

		}		
	}
}

bool CSpatialTree::SAH(uint8 exeFlags, SInterval * pInterval, TreeNode & s, uint32 & axis, int32 & sah_bin, uint32 nPrim)
{
	if (exeFlags == EXEC_H)
	{
		S_Box box;	

		s.split = 0.0f;
		axis = TREE_LEAF;
		sah_bin = 0;

		float Cb = 1.0f, Ct = 1.0f;
			
		uint32 nBin = m_histopyramid[X].nBin();

		float2 * pPrimCount = NULL;

		float length[XYZ] = { pInterval[X].end - pInterval[X].start, 
			                    pInterval[Y].end - pInterval[Y].start, 
											    pInterval[Z].end - pInterval[Z].start };

		float step  [XYZ] = { length[X] / nBin, 
			                    length[Y] / nBin, 
												  length[Z] / nBin };

		AABox(&box, (float2 *) pInterval);

		float surface = Surface(box);
		float inv_surface = 1.0f / surface;

		float oldSAH = Cb + Ct * nPrim;

		for (uint32 iAxis = X; iAxis <= Z; iAxis++)
		{
			pPrimCount = m_histopyramid[iAxis].scan.hPtrR();
		
			float splitPoint;

			for (uint32 iBin = 1; iBin < nBin-1; iBin++)
			{
				S_Box leftBox, rightBox;

				splitPoint = pInterval[iAxis].start + step[iAxis] * iBin;

				leftBox  = box;
				rightBox = box;

				((float *)&( leftBox.pmax))[iAxis] = splitPoint; // change pmax for left box 
				((float *)&(rightBox.pmin))[iAxis] = splitPoint; // and    pmin for rght box

				float leftSAH  = (Ct * pPrimCount[iBin].x) * Surface(leftBox);
				float rightSAH = (Ct * pPrimCount[iBin].y) * Surface(rightBox);

				float newSAH = (leftSAH + rightSAH) * inv_surface + Cb*2;

				//////////////////////////////////////////////////////////////////////////
				/// Check is newSAH is better cmpd to oldSAH
				//////////////////////////////////////////////////////////////////////////
				if (newSAH < oldSAH)
				{
					oldSAH = newSAH;
					// we found a better SAH so s is definitely not a LEAF anymore
					s.split = splitPoint;
					s.sah = newSAH;
					axis = iAxis;
					sah_bin = iBin;
					
					//////////////////////////////////////////////////////////////////////////
					/// Save how many elements go into each subtree
					//////////////////////////////////////////////////////////////////////////
					s.left = pPrimCount[iBin].x;
					s.right = pPrimCount[iBin].y;
				}
			}
		}
		// if we have found a better subdivision axis is not equal to LEAF
		return axis != TREE_LEAF;
	}

	return false;
}

void CSpatialTree::KdTree_p2(TreeNode *pb, KdNode * pt, TreeNode & bn)
{
	uint32 currTraversalIdx = m_nTraversalIdx;

	m_nTraversalIdx++;

	if (!bn.Leaf())
	{			
		//////////////////////////////////////////////////////////////////////////
		/// for current node save split position
		//////////////////////////////////////////////////////////////////////////
		pt[currTraversalIdx].split = bn.split;
		
		//////////////////////////////////////////////////////////////////////////
		/// Insert left subTREE
		//////////////////////////////////////////////////////////////////////////
		KdTree_p2(pb, pt, pb[bn.left]);

		//////////////////////////////////////////////////////////////////////////
		/// After left subTREE is inserted m_nTravIdx holds offset for right subtree for current node
		//////////////////////////////////////////////////////////////////////////
		pt[currTraversalIdx].right = m_nTraversalIdx;

		//////////////////////////////////////////////////////////////////////////
		/// Insert left subTREE
		//////////////////////////////////////////////////////////////////////////
		KdTree_p2(pb, pt, pb[bn.right]);
	}
	else
	{
		pt[currTraversalIdx].count = ((bn.end - bn.start) << 2) | TREE_LEAF;
 		pt[currTraversalIdx].start = bn.start;
	}
}

void CSpatialTree::KdTree_p1(uint8 exeFlags)
{
	if (exeFlags == EXEC_H)
	{
		//////////////////////////////////////////////////////////////////////////
		/// Take Root Node and convert it from BuildNode to TraversalNode
		//////////////////////////////////////////////////////////////////////////
		TreeNode *pTree   = (TreeNode *) m_Tree.hPtrR();
		KdNode   *pKdNode = (KdNode   *) m_KdTree.hPtrR();

		TreeNode root = pTree[TREE_ROOT];

		m_nTraversalIdx = TREE_ROOT;

		KdTree_p2(pTree, pKdNode, root);
	}

	if (EXEC_D)
	{
		//////////////////////////////////////////////////////////////////////////
		/// TODO : This is so sequential I guess there will be no EXEC_D
		//////////////////////////////////////////////////////////////////////////
	}
}

void CSpatialTree::KdTree_p0(uint8 exeFlags)
{
	CBenchmark buildtreeBench;
	
	buildtreeBench.Start("KdTree_p0");

	if (exeFlags == EXEC_H)
	{
		//////////////////////////////////////////////////////////////////////////
		/// Take Root Node and insert it into the BuildTree Node Tree
		//////////////////////////////////////////////////////////////////////////

		//////////////////////////////////////////////////////////////////////////
		/// First init index array for root
		//////////////////////////////////////////////////////////////////////////
		uint32 * pPrimIdxR = m_primIdx.hPtrR();
		uint32 * pPrimIdxW = m_primIdx.hPtrW();
		uint32 * pTreeIdx  = m_treeIdx.hPtrR();

		for (uint32 iPrim = 0; iPrim < m_nFace; iPrim++) pPrimIdxR[iPrim] = iPrim;

		//////////////////////////////////////////////////////////////////////////
		/// Init Root Node
		//////////////////////////////////////////////////////////////////////////
		m_nNodeIndex = 0; 
		m_nPrimitive = 0;
		m_nTree = 0;
		m_nInterval = 0;		
		
		TreeNode root(m_nPrimitive, m_nFace);

		TreeNode *pTree = (TreeNode *) m_Tree.hPtrR();
		pTree[m_nNodeIndex++] = root;
		
		SInterval *pInterval = (SInterval *) m_Interval.hPtrR();
		pInterval[m_nInterval++] = f2(m_box.pmin.x, m_box.pmax.x);
		pInterval[m_nInterval++] = f2(m_box.pmin.y, m_box.pmax.y);
		pInterval[m_nInterval++] = f2(m_box.pmin.z, m_box.pmax.z);

		//////////////////////////////////////////////////////////////////////////
		/// Get Maximum size of primIdx array
		//////////////////////////////////////////////////////////////////////////
		uint32 maxPrim = m_primIdx.size();

		//////////////////////////////////////////////////////////////////////////
		/// Try to create maxDepth levels of KdTree
		//////////////////////////////////////////////////////////////////////////
		int32 depth = 0;
		int32 leaf_count = 0;
		int32 node_count = 1, node_offset = 0;
		
		while (depth < m_nMaxDepth && leaf_count != node_count)
		{
			pPrimIdxR = m_primIdx.hPtrR();
			pPrimIdxW = m_primIdx.hPtrW();
			
			// reset primitive_idx_offset
			m_nPrimitive = 0; 
			
			// reset leaf_count 
			leaf_count = 0; 

			// this level has this many nodes that require splitting
			node_count = (depth > 0) ? 1 << depth : 1;             
 
			// nodes for this level are located starting from this offset
			node_offset = ((1 << (depth+1)) - 1) - node_count;     

			//////////////////////////////////////////////////////////////////////////
			/// loop over nodes from node_offset to node_offset + node_count
			///		Take 'i' node
			///			Try Computing SAH on this Node using m_primIdx.R
			///			If SAH is available add two new nodes
			///       Update existing Node as NON LEAF
			///				Sort & Copy Indexes into m_primIdx.W
			///			If SAH is unavailable add new nodes marked as non valid
			///   Swap R & W in m_primIdx
			///   Proceed to next depth level
			//////////////////////////////////////////////////////////////////////////
			for (int32 iNode = 0; iNode < node_count; iNode++)
			{
				int32 current_node_index = node_offset + iNode;

				TreeNode current_node = pTree[current_node_index];

				int32   primitive_count = current_node.end - current_node.start;

				//////////////////////////////////////////////////////////////////////////
				/// take iNode from node_offset offset and check that there is enough memory:
				///		nTreeIdx - shows how many elements are already copied into treeIdx
				///		nPrimIdx - shows how many elements are already copied into primIdx
				///		at worst the number of primitives can increase by primitive_count*2
				/// and if there is that much memory left check that node requires a split			
				//////////////////////////////////////////////////////////////////////////
				if ((m_nTree+m_nPrimitive+primitive_count*2 < maxPrim) && !current_node.Leaf())
				{							
					SInterval * pCurInterval = pInterval + current_node_index*XYZ;
					//////////////////////////////////////////////////////////////////////////
					/// Compute Histogram
					//////////////////////////////////////////////////////////////////////////
					Histogram(EXEC_H, pPrimIdxR + current_node.start, primitive_count, pCurInterval);

					//////////////////////////////////////////////////////////////////////////
					/// Compute SAH
					//////////////////////////////////////////////////////////////////////////
					TreeNode s;
					uint32 axis = TREE_LEAF;
					int32  bin = 0;

					if (primitive_count > m_nMaxPrim && (depth+1<m_nMaxDepth) && SAH(EXEC_H, pCurInterval, s, axis, bin, primitive_count))
					{
						//////////////////////////////////////////////////////////////////////////
						/// For current Node set splitting plane
						//////////////////////////////////////////////////////////////////////////
						pTree[current_node_index].SplitAxis(s.split, axis); //split = as_float((as_int(s.split) & (~KD_LEAF)) | axis);
						pTree[current_node_index].left  = m_nNodeIndex;   
						pTree[current_node_index].right = m_nNodeIndex+1;

						//////////////////////////////////////////////////////////////////////////
						/// Select Primitive Bounding Box array according to axis
						//////////////////////////////////////////////////////////////////////////
						float2 * pBI = m_PrimitiveBI[axis].hPtrR();

						//////////////////////////////////////////////////////////////////////////
						/// Make Left and Right Nodes and insert them into buildTree
						//////////////////////////////////////////////////////////////////////////
						TreeNode left_node, right_node;

						//////////////////////////////////////////////////////////////////////////
						/// BuildTree will try to split left node again on the next depth iter-n
						/// currently there are primIdx elements written into pPrimIdx array
						/// left node stores s.left elements
						/// parent of left node is current node with it's offset
						//////////////////////////////////////////////////////////////////////////
						int32 indexLeft = m_nPrimitive;

						float interval_length = pCurInterval[axis].length();
						float min_length = interval_length;
						float refined_split = s.split;												
						
						for (int32 iPrim = 0; iPrim < primitive_count; iPrim++)
						{
							uint32 PrimitiveIndex = pPrimIdxR[current_node.start + iPrim];
							float2 PrimitiveBI = pBI[PrimitiveIndex];							
							// due to floating point precision this comparison doesn't work well whenever interval length is 0.000xy small
							//if (PrimitiveBI.x < s.split) pPrimIdxW[indexLeft++] = PrimitiveIndex;						
							// so instead I compute histogram bin and compare it against sah bin returned from SAH function
							//int32 primitive_bin = (PrimitiveBI.x - interval[axis].start) / interval_length * m_histopyramid[axis].nBin();
							//if (primitive_bin < bin) pPrimIdxW[indexLeft++] = PrimitiveIndex;
							
							// if primitive starts before refined split
							// or happens to be an axis aligned triangle
							if (PrimitiveBI.x <= refined_split)
								pPrimIdxW[indexLeft++] = PrimitiveIndex;
						}
						
						int32 indexRight = indexLeft;

						for (int32 iPrim = 0; iPrim < primitive_count; iPrim++)
						{
							uint32 PrimitiveIndex = pPrimIdxR[current_node.start + iPrim];
							float2 PrimitiveBI = pBI[PrimitiveIndex];						
							// due to floating point precision this comparison doesn't work well whenever interval length is 0.000xy small
							//if (PrimitiveBI.y >= s.split) pPrimIdxW[indexRight++] = PrimitiveIndex;		
							// so instead I compute histogram bin and compare it against sah bin returned from SAH function
							//int32 primitive_bin = (PrimitiveBI.y - interval[axis].start) / interval_length * m_histopyramid[axis].nBin();							
							//if (primitive_bin >= bin) pPrimIdxW[indexRight++] = PrimitiveIndex;
											
							if (PrimitiveBI.y > refined_split)
								pPrimIdxW[indexRight++] = PrimitiveIndex;
						}
						
						int32 left_count, right_count, total_count;

						left_node.start = m_nPrimitive;
						left_node.end = indexLeft;
						left_count = left_node.end - left_node.start;						
						left_node.parent = current_node_index;
						if (left_count == 0) left_node.SplitAxis(0.0f, TREE_LEAF); // .split = as_float(KD_LEAF);

						right_node.start = indexLeft;
						right_node.end = indexRight;
						right_count = right_node.end - right_node.start;
						right_node.parent = current_node_index;
						if (right_count == 0) right_node.SplitAxis(0.0f, TREE_LEAF); //.split = as_float(KD_LEAF);
						
						total_count = left_count + right_count;

						pTree[m_nNodeIndex++] = left_node;
						pTree[m_nNodeIndex++] = right_node;

						//////////////////////////////////////////////////////////////////////////
						/// Make Left and Right Intervals and insert them into intervalTree
						//////////////////////////////////////////////////////////////////////////
						SInterval left_interval[XYZ], right_interval[XYZ];
						
						// copy node interval into left_interval
						left_interval[X] = pCurInterval[X]; 
						left_interval[Y] = pCurInterval[Y]; 
						left_interval[Z] = pCurInterval[Z]; 
						// copy node interval into right_interval
						right_interval[X] = pCurInterval[X]; 
						right_interval[Y] = pCurInterval[Y]; 
						right_interval[Z] = pCurInterval[Z]; 

						// modify left & right ends of interval on split axis
						pTree[current_node_index].SplitAxis(refined_split, axis); // .split = as_float(as_uint(refined_split) & (~KD_LEAF) | axis);
						left_interval[axis].end = refined_split;
						right_interval[axis].start = refined_split; 

						pInterval[m_nInterval++] = left_interval[X];
						pInterval[m_nInterval++] = left_interval[Y];
						pInterval[m_nInterval++] = left_interval[Z];
						pInterval[m_nInterval++] = right_interval[X];
						pInterval[m_nInterval++] = right_interval[Y];
						pInterval[m_nInterval++] = right_interval[Z];

						m_nPrimitive += total_count;
					}
					else
					{
						//////////////////////////////////////////////////////////////////////////
						/// Mark as not needing splitting anymore and update index position
						/// Insert dummy buildNode twice
						/// Copy Elements to treeIdx array
						//////////////////////////////////////////////////////////////////////////
						pTree[current_node_index].start = m_nTree;
						pTree[current_node_index].end   = m_nTree + primitive_count;
						pTree[current_node_index].SplitAxis(0.0f, TREE_LEAF); // .split = as_float(KD_LEAF);

						TreeNode dummy_node(as_float(TREE_LEAF), current_node_index);  // no splitting for child nodes as well

						pTree[m_nNodeIndex++] = dummy_node; // left  child dummy
						pTree[m_nNodeIndex++] = dummy_node; // right child dummy
 
						SInterval dummy_interval[XYZ];
						
						// copy node interval into dummy_node
						dummy_interval[X] = pCurInterval[X]; 
						dummy_interval[Y] = pCurInterval[Y]; 
						dummy_interval[Z] = pCurInterval[Z]; 

						pInterval[m_nInterval++] = dummy_interval[X];
						pInterval[m_nInterval++] = dummy_interval[Y];
						pInterval[m_nInterval++] = dummy_interval[Z];
						pInterval[m_nInterval++] = dummy_interval[X];
						pInterval[m_nInterval++] = dummy_interval[Y];
						pInterval[m_nInterval++] = dummy_interval[Z];

						for (uint32 iTreeIdx = current_node.start; iTreeIdx < current_node.end; iTreeIdx++)
							pTreeIdx[m_nTree++] = pPrimIdxR[iTreeIdx];

						leaf_count++; // if all nodes kick this line - next depth will not be checked
					} // if (primitive_count > 4 && SAH(EXEC_H, pInterval + current_node_index*XYZ, s, primitive_count))
				} // if ((m_nTree+m_nPrimitive+primitive_count*2 < maxPrim) && !current_node.Leaf())
				else 
				{			
					TreeNode dummy_node(as_float(TREE_LEAF), current_node_index);  // no splitting for child nodes as well

					pTree[m_nNodeIndex++] = dummy_node; // left  child dummy
					pTree[m_nNodeIndex++] = dummy_node; // right child dummy

					SInterval dummy_interval[XYZ];

					pInterval[m_nInterval++] = dummy_interval[X];
					pInterval[m_nInterval++] = dummy_interval[Y];
					pInterval[m_nInterval++] = dummy_interval[Z];
					pInterval[m_nInterval++] = dummy_interval[X];
					pInterval[m_nInterval++] = dummy_interval[Y];
					pInterval[m_nInterval++] = dummy_interval[Z];

					leaf_count += 1; // if all nodes kick this line - next depth will not be checked
				}
			} // for (uint32 iNode = 0; iNode < node_count; iNode++)
			m_primIdx.Swap();
			
			depth++; // increase depth level
			
			//fprintf(stderr, "\nDepth Level %d\n", depth); 
		} // while (depth < m_nMaxDepth)
	}// if (exeFlags == EXEC_H)

	buildtreeBench.End();
	buildtreeBench.Print();

	if (exeFlags == EXEC_D) 	
	{
		//////////////////////////////////////////////////////////////////////////
		/// TODO : either add a different path or insert Histograms and Scan into EXEC_H
		//////////////////////////////////////////////////////////////////////////
	}
}

void CSpatialTree::BVHTree_p2(TreeNode *pb, BVHNode * pn, SInterval * pi, TreeNode & bn, int i)
{
	uint32 currTraversalIdx = m_nTraversalIdx;

	m_nTraversalIdx++;

	if (!bn.Leaf())
	{			
		//////////////////////////////////////////////////////////////////////////
		/// for current node save split position
		//////////////////////////////////////////////////////////////////////////
		pn[currTraversalIdx].pmin.x = pi[i * XYZ + X].start;
		pn[currTraversalIdx].pmax.x = pi[i * XYZ + X].end;

		pn[currTraversalIdx].pmin.y = pi[i * XYZ + Y].start;
		pn[currTraversalIdx].pmax.y = pi[i * XYZ + Y].end;

		pn[currTraversalIdx].pmin.z = pi[i * XYZ + Z].start;
		pn[currTraversalIdx].pmax.z = pi[i * XYZ + Z].end;

		pn[currTraversalIdx].split = bn.split;

		//////////////////////////////////////////////////////////////////////////
		/// Insert left subTREE
		//////////////////////////////////////////////////////////////////////////
		BVHTree_p2(pb, pn, pi, pb[bn.left], bn.left);

		//////////////////////////////////////////////////////////////////////////
		/// After left subTREE is inserted m_nTravIdx holds offset for right subtree for current node
		//////////////////////////////////////////////////////////////////////////
		pn[currTraversalIdx].right = m_nTraversalIdx;

		//////////////////////////////////////////////////////////////////////////
		/// Insert left subTREE
		//////////////////////////////////////////////////////////////////////////
		BVHTree_p2(pb, pn, pi, pb[bn.right], bn.right);
	}
	else
	{
		pn[currTraversalIdx].pmin.x = pi[i * XYZ + X].start;
		pn[currTraversalIdx].pmax.x = pi[i * XYZ + X].end;

		pn[currTraversalIdx].pmin.y = pi[i * XYZ + Y].start;
		pn[currTraversalIdx].pmax.y = pi[i * XYZ + Y].end;

		pn[currTraversalIdx].pmin.z = pi[i * XYZ + Z].start;
		pn[currTraversalIdx].pmax.z = pi[i * XYZ + Z].end;

		pn[currTraversalIdx].count = ((bn.end - bn.start) << 2) | TREE_LEAF;
		pn[currTraversalIdx].start = bn.start;
	}
}

void CSpatialTree::BVHTree_p1(uint8 exeFlags)
{
	if (exeFlags == EXEC_H)
	{
		//////////////////////////////////////////////////////////////////////////
		/// Take Root Node and convert it from BuildNode to TraversalNode
		//////////////////////////////////////////////////////////////////////////
		TreeNode  *pTree     = (TreeNode  *) m_Tree.hPtrR();
		BVHNode   *pBVHNode  = (BVHNode   *) m_BVHTree.hPtrR();
		SInterval *pInterval = (SInterval *) m_Interval.hPtrR();

		TreeNode root = pTree[TREE_ROOT];

		m_nTraversalIdx = TREE_ROOT;

		BVHTree_p2(pTree, pBVHNode, pInterval, root, TREE_ROOT);
	}

	if (EXEC_D)
	{
		//////////////////////////////////////////////////////////////////////////
		/// TODO : This is so sequential I guess there will be no EXEC_D
		//////////////////////////////////////////////////////////////////////////
	}
}

void CSpatialTree::BVHTree_p0(uint8 exeFlags)
{
	CBenchmark buildtreeBench;
	
	buildtreeBench.Start("BVHTree_p0");

	if (exeFlags == EXEC_H)
	{
		//////////////////////////////////////////////////////////////////////////
		/// First init index array for root
		//////////////////////////////////////////////////////////////////////////
		uint32 * pPrimIdxR = m_primIdx.hPtrR();
		uint32 * pPrimIdxW = m_primIdx.hPtrW();
		uint32 * pTreeIdx  = m_treeIdx.hPtrR();

		for (uint32 iPrim = 0; iPrim < m_nFace; iPrim++) pPrimIdxR[iPrim] = iPrim;

		//////////////////////////////////////////////////////////////////////////
		/// Init Root Node
		//////////////////////////////////////////////////////////////////////////
		TreeNode root(m_nPrimitive, m_nFace);

		m_nNodeIndex = 0; 
		m_nPrimitive = 0;
		m_nTree = 0;
		m_nInterval = 0;

		TreeNode * pTree = (TreeNode *) m_Tree.hPtrR();
		pTree[m_nNodeIndex++] = root;
		
		SInterval * pInterval = (SInterval *) m_Interval.hPtrR();
		pInterval[m_nInterval++] = f2(m_box.pmin.x, m_box.pmax.x);
		pInterval[m_nInterval++] = f2(m_box.pmin.y, m_box.pmax.y);
		pInterval[m_nInterval++] = f2(m_box.pmin.z, m_box.pmax.z);

		//////////////////////////////////////////////////////////////////////////
		/// Get Maximum size of primIdx array
		//////////////////////////////////////////////////////////////////////////
		uint32 maxPrim = m_primIdx.size();

		//////////////////////////////////////////////////////////////////////////
		/// Try to create maxDepth levels of KdTree
		//////////////////////////////////////////////////////////////////////////
		int32 depth = 0;
		int32 leaf_count = 0;
		int32 node_count = 1, node_offset = 0;
		
		while (depth < m_nMaxDepth && leaf_count != node_count)
		{
			pPrimIdxR = m_primIdx.hPtrR();
			pPrimIdxW = m_primIdx.hPtrW();
			
			// reset primitive_idx_offset
			m_nPrimitive = 0; 
			
			// reset leaf_count 
			leaf_count = 0; 

			// this level has this many nodes that require splitting
			node_count = (depth > 0) ? 1 << depth : 1;             
 
			// nodes for this level are located starting from this offset
			node_offset = ((1 << (depth+1)) - 1) - node_count;     

			//////////////////////////////////////////////////////////////////////////
			/// loop over nodes from node_offset to node_offset + node_count
			///		Take 'i' node
			///			Try Computing SAH on this Node using m_primIdx.R
			///			If SAH is available add two new nodes
			///       Update existing Node as NON LEAF
			///				Sort & Copy Indexes into m_primIdx.W
			///			If SAH is unavailable add new nodes marked as non valid
			///   Swap R & W in m_primIdx
			///   Proceed to next depth level
			//////////////////////////////////////////////////////////////////////////
			for (int32 iNode = 0; iNode < node_count; iNode++)
			{
				int32 current_node_index = node_offset + iNode;

				TreeNode current_node = pTree[current_node_index];

				int32   primitive_count = current_node.end - current_node.start;

				//////////////////////////////////////////////////////////////////////////
				/// take iNode from node_offset offset and check that there is enough memory:
				///		nTreeIdx - shows how many elements are already copied into treeIdx
				///		nPrimIdx - shows how many elements are already copied into primIdx
				///		at worst the number of primitives can increase by primitive_count*2
				/// and if there is that much memory left check that node requires a split			
				//////////////////////////////////////////////////////////////////////////
				if ((m_nTree+m_nPrimitive+primitive_count*2 < maxPrim) && !current_node.Leaf())
				{							
					SInterval * pCurInterval = pInterval + current_node_index*XYZ;
					//////////////////////////////////////////////////////////////////////////
					/// Compute Histogram
					//////////////////////////////////////////////////////////////////////////
					Histogram(EXEC_H, pPrimIdxR + current_node.start, primitive_count, pCurInterval);

					//////////////////////////////////////////////////////////////////////////
					/// Compute SAH
					//////////////////////////////////////////////////////////////////////////
					TreeNode s;
					uint32 axis = TREE_LEAF;
					int32  bin = 0;

					if (primitive_count > m_nMaxPrim && (depth+1<m_nMaxDepth) && SAH(EXEC_H, pCurInterval, s, axis, bin, primitive_count))
					{
						//////////////////////////////////////////////////////////////////////////
						/// For current Node set splitting plane
						//////////////////////////////////////////////////////////////////////////
						pTree[current_node_index].SplitAxis(s.split, axis); // split = as_float((as_uint(s.split) & (~KD_LEAF)) | axis);
						pTree[current_node_index].left  = m_nNodeIndex;   
						pTree[current_node_index].right = m_nNodeIndex+1;

						//////////////////////////////////////////////////////////////////////////
						/// Select Primitive Bounding Box array according to axis
						//////////////////////////////////////////////////////////////////////////
						float2 * pBI[XYZ] = { m_PrimitiveBI[0].hPtrR(), m_PrimitiveBI[1].hPtrR(), m_PrimitiveBI[2].hPtrR() };

						//////////////////////////////////////////////////////////////////////////
						/// Make Left and Right Nodes and insert them into buildTree
						//////////////////////////////////////////////////////////////////////////
						TreeNode left_node, right_node;

						//////////////////////////////////////////////////////////////////////////
						/// BuildTree will try to split left node again on the next depth iter-n
						/// currently there are primIdx elements written into pPrimIdx array
						/// left node stores s.left elements
						/// parent of left node is current node with it's offset
						//////////////////////////////////////////////////////////////////////////
						uint32 indexLeft  = m_nPrimitive;
						uint32 left_sum_cmp = 0;
						uint32 right_sum_cmp = 0;

						float interval_length = pCurInterval[axis].length();

						float min_length = interval_length;
						float refined_split = s.split;												
						float right_split = s.split;		

						//////////////////////////////////////////////////////////////////////////
						/// Make Left and Right Intervals and insert them into intervalTree
						//////////////////////////////////////////////////////////////////////////
						SInterval left_interval[XYZ], right_interval[XYZ];						
						// copy node interval into left_interval
						left_interval[X] = pCurInterval[X]; 
						left_interval[Y] = pCurInterval[Y]; 
						left_interval[Z] = pCurInterval[Z]; 
						// copy node interval into right_interval
						right_interval[X] = pCurInterval[X]; 
						right_interval[Y] = pCurInterval[Y]; 
						right_interval[Z] = pCurInterval[Z];

						// modify left / right ends of interval on split axis
						left_interval[axis].end = refined_split; 
						right_interval[axis].start = refined_split; 
						
						// during BVH construction bounding box can be recalculated during primitive sorting					
						bool first = true;
						float2 search_interval[XYZ] = { f2(), f2(), f2() } ;
						for (int32 iPrim = 0; iPrim < primitive_count; iPrim++)
						{
							uint32 PrimitiveIndex = pPrimIdxR[current_node.start + iPrim];
							float2 PrimitiveBI = pBI[axis][PrimitiveIndex];							
							// due to floating point precision this comparison doesn't work well whenever interval length is 0.000xy small
							//if (PrimitiveBI.x < s.split) pPrimIdxW[indexLeft++] = PrimitiveIndex;						
							// so instead I compute histogram bin and compare it against sah bin returned from SAH function
							//int32 primitive_bin = (PrimitiveBI.x - interval[axis].start) / interval_length * m_histopyramid[axis].nBin();
							//if (primitive_bin < bin) pPrimIdxW[indexLeft++] = PrimitiveIndex;
							//left_sum_cmp += (PrimitiveBI.x == s.split);
							
							if (PrimitiveBI.x < refined_split)
							{
								pPrimIdxW[indexLeft++] = PrimitiveIndex;
								
								// when first primitive falls into left subtree search_interval 
								// chooses it's boundaries according to this primitive and 
								// marks first as false
								if (first)
								{
									search_interval[0] = pBI[0][PrimitiveIndex];
									search_interval[1] = pBI[1][PrimitiveIndex];
									search_interval[2] = pBI[2][PrimitiveIndex];
									first = false;
								}
								// if first primitive has been found bounding box should be computed
								// by taking min and max of the interval along each axis
								else
								{
									for (int iAxis = 0; iAxis < W; iAxis++)
									{
										search_interval[iAxis].x = min(search_interval[iAxis].x, pBI[iAxis][PrimitiveIndex].x);
										search_interval[iAxis].y = max(search_interval[iAxis].y, pBI[iAxis][PrimitiveIndex].y);
									}
								}
							}
						}
						
						// if first has been updated within the sorting loop 
						// maybe there is a better bounding box 
						if (!first)
						{
							// but it has to be saturated to the original bounding box
							for (int iAxis = 0; iAxis < W; iAxis++)
							{
								left_interval[iAxis].start = max(left_interval[iAxis].start, search_interval[iAxis].x);
								left_interval[iAxis].end   = min(left_interval[iAxis].end,   search_interval[iAxis].y);
							}
						}

						first = true;
						
						uint32 indexRight = indexLeft;

						for (int32 iPrim = 0; iPrim < primitive_count; iPrim++)
						{
							uint32 PrimitiveIndex = pPrimIdxR[current_node.start + iPrim];
							float2 PrimitiveBI = pBI[axis][PrimitiveIndex];						
							// due to floating point precision this comparison doesn't work well whenever interval length is 0.000xy small
							//if (PrimitiveBI.y >= s.split) pPrimIdxW[indexRight++] = PrimitiveIndex;		
							// so instead I compute histogram bin and compare it against sah bin returned from SAH function
							//int32 primitive_bin = (PrimitiveBI.y - interval[axis].start) / interval_length * m_histopyramid[axis].nBin();							
							//if (primitive_bin >= bin) pPrimIdxW[indexRight++] = PrimitiveIndex;											 

							//right_sum_cmp += (PrimitiveBI.y == s.split);
							if (PrimitiveBI.y > refined_split)
							{
								pPrimIdxW[indexRight++] = PrimitiveIndex;
								
								// when first primitive falls into left subtree search_interval 
								// chooses it's boundaries according to this primitive and 
								// marks first as false
								if (first)
								{
									search_interval[0] = pBI[0][PrimitiveIndex];
									search_interval[1] = pBI[1][PrimitiveIndex];
									search_interval[2] = pBI[2][PrimitiveIndex];
									first = false;
								}
								// if first primitive has been found bounding box should be computed
								// by taking min and max of the interval along each axis
								else
								{
									for (int iAxis = 0; iAxis < W; iAxis++)
									{
										search_interval[iAxis].x = min(search_interval[iAxis].x, pBI[iAxis][PrimitiveIndex].x);
										search_interval[iAxis].y = max(search_interval[iAxis].y, pBI[iAxis][PrimitiveIndex].y);
									}
								}
							}
						}

					  // if first has been updated within the sorting loop 
						// maybe there is a better bounding box 
						if (!first)
						{
							// but it has to be saturated to the original bounding box
							for (int iAxis = 0; iAxis < W; iAxis++)
							{
								right_interval[iAxis].start = max(right_interval[iAxis].start, search_interval[iAxis].x);
								right_interval[iAxis].end   = min(right_interval[iAxis].end,   search_interval[iAxis].y);
							}
						}
						
						int32 left_count, right_count, total_count;

						left_node.start = m_nPrimitive;
						left_node.end = indexLeft;
						left_count = left_node.end - left_node.start;						
						left_node.parent = current_node_index;
						if (left_count == 0) left_node.SplitAxis(0.0f, TREE_LEAF); // split = as_float(KD_LEAF);

						right_node.start = indexLeft;
						right_node.end = indexRight;
						right_count = right_node.end - right_node.start;
						right_node.parent = current_node_index;
						if (right_count == 0) right_node.SplitAxis(0.0f, TREE_LEAF); //split = as_float(KD_LEAF);
						
						total_count = left_count + right_count;

						pTree[m_nNodeIndex++] = left_node;
						pTree[m_nNodeIndex++] = right_node;

						pTree[current_node_index].SplitAxis(refined_split, axis); // .split = as_float(as_uint(refined_split) & (~KD_LEAF) | axis);

						pInterval[m_nInterval++] = left_interval[X];
						pInterval[m_nInterval++] = left_interval[Y];
						pInterval[m_nInterval++] = left_interval[Z];
						pInterval[m_nInterval++] = right_interval[X];
						pInterval[m_nInterval++] = right_interval[Y];
						pInterval[m_nInterval++] = right_interval[Z];

						m_nPrimitive += total_count;
					}
					else
					{
						//////////////////////////////////////////////////////////////////////////
						/// Mark as not needing splitting anymore and update index position
						/// Insert dummy buildNode twice
						/// Copy Elements to treeIdx array
						//////////////////////////////////////////////////////////////////////////
						pTree[current_node_index].start = m_nTree;
						pTree[current_node_index].end   = m_nTree + primitive_count;
						pTree[current_node_index].SplitAxis(0.0f, TREE_LEAF); //split = as_float(KD_LEAF);

						TreeNode dummy_node(as_float(TREE_LEAF), current_node_index);  // no splitting for child nodes as well

						pTree[m_nNodeIndex++] = dummy_node; // left  child dummy
						pTree[m_nNodeIndex++] = dummy_node; // right child dummy
 
						SInterval dummy_interval[XYZ];
						
						// copy node interval into dummy_node
						dummy_interval[X] = pCurInterval[X]; 
						dummy_interval[Y] = pCurInterval[Y]; 
						dummy_interval[Z] = pCurInterval[Z]; 

						pInterval[m_nInterval++] = dummy_interval[X];
						pInterval[m_nInterval++] = dummy_interval[Y];
						pInterval[m_nInterval++] = dummy_interval[Z];
						pInterval[m_nInterval++] = dummy_interval[X];
						pInterval[m_nInterval++] = dummy_interval[Y];
						pInterval[m_nInterval++] = dummy_interval[Z];

						for (uint32 iTreeIdx = current_node.start; iTreeIdx < current_node.end; iTreeIdx++)
							pTreeIdx[m_nTree++] = pPrimIdxR[iTreeIdx];

						leaf_count++; // if all nodes kick this line - next depth will not be checked
					} // if (primitive_count > 4 && SAH(EXEC_H, pInterval + current_node_index*XYZ, s, primitive_count))
				} // if ((m_nTree+m_nPrimitive+primitive_count*2 < maxPrim) && !current_node.Leaf())
				else 
				{
					leaf_count += 1;			

					TreeNode dummy_node(as_float(TREE_LEAF), current_node_index);  // no splitting for child nodes as well

					pTree[m_nNodeIndex++] = dummy_node; // left  child dummy
					pTree[m_nNodeIndex++] = dummy_node; // right child dummy

					SInterval dummy_interval[XYZ];

					pInterval[m_nInterval++] = dummy_interval[X];
					pInterval[m_nInterval++] = dummy_interval[Y];
					pInterval[m_nInterval++] = dummy_interval[Z];
					pInterval[m_nInterval++] = dummy_interval[X];
					pInterval[m_nInterval++] = dummy_interval[Y];
					pInterval[m_nInterval++] = dummy_interval[Z];
				}
			} // for (uint32 iNode = 0; iNode < node_count; iNode++)
			m_primIdx.Swap();
			
			depth++; // increase depth level
			
		} // while (depth < m_nMaxDepth)
	}// if (exeFlags == EXEC_H)

	buildtreeBench.End();
	buildtreeBench.Print();

	if (exeFlags == EXEC_D) 	
	{
		//////////////////////////////////////////////////////////////////////////
		/// TODO : either add a different path or insert Histograms and Scan into EXEC_H
		//////////////////////////////////////////////////////////////////////////
	}
}

float2 *        CSpatialTree::pKdTree()
{
	return m_KdTree.hPtrR();
}

float4 *        CSpatialTree::pBVHTree()
{
	return m_BVHTree.hPtrR();
}

uint   *        CSpatialTree::pPrimitive()
{
	return m_treeIdx.hPtrR();
}

int    *        CSpatialTree::pFace()
{
	return m_pPosIdx->hPtrW();
}

float4 *        CSpatialTree::pVertex()
{
	return m_pPos->hPtrW();
}

int32           CSpatialTree::maxNode()
{
	return m_nTraversalIdx;
}

//////////////////////////////////////////////////////////////////////////
/// Get SAABox
//////////////////////////////////////////////////////////////////////////
S_Box           CSpatialTree::box()
{
	return m_box;
}